#include "Including.h"

GLfloat x_force = 0;
GLfloat y_force = 0;
GLfloat z_force = 0;


namespace Bullet{

bool collisionFlag = false;

btBroadphaseInterface* broadphase;
btDefaultCollisionConfiguration* collisionConfiguration;
btCollisionDispatcher* dispatcher;
btSequentialImpulseConstraintSolver* solver;
btDiscreteDynamicsWorld* dynamicsWorld;

btCollisionShape* groundShape;
btCollisionShape* triggerShape;
btCollisionShape* fallShape;
btCollisionShape* fallShape2;

btDefaultMotionState* groundMotionState;
btDefaultMotionState* triggerMotionState;
btDefaultMotionState* fallMotionState;
btDefaultMotionState* fallMotionState2;

btRigidBody* groundRigidBody;
btRigidBody* triggerRigidBody;
btRigidBody* fallRigidBody;
btRigidBody* fallRigidBody2;

//{ground, fallrigidbody, fallrigidbody2, trigger}
enum ObjectsIds{ ACTOR0, ACTOR1, ACTOR2, ACTOR3 };
//store collision objects
collisionObject body[10];


void MyBullet::initPhysics()
{
	 
	broadphase = new btDbvtBroadphase();
	collisionConfiguration = new btDefaultCollisionConfiguration();
	dispatcher = new btCollisionDispatcher(collisionConfiguration);
	solver = new btSequentialImpulseConstraintSolver;
 
	//Contact Callback
	gContactAddedCallback = CustomMaterialCombinerCallback;

    dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver,collisionConfiguration);
    dynamicsWorld->setGravity(btVector3(0,-5,0));
 
 
    groundShape = new btStaticPlaneShape(btVector3(0,1,0),1);
    triggerShape = new btCapsuleShape(0.5,9);
    fallShape = new btSphereShape(0.5);
 	fallShape2 = new btSphereShape(1);

  
	//GROUND: GroundRigidBody
    groundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,-6,0)));
	btRigidBody::btRigidBodyConstructionInfo groundRigidBodyCI(0,groundMotionState,groundShape,btVector3(0,0,0));
    groundRigidBody = new btRigidBody(groundRigidBodyCI);
	groundRigidBody->getCollisionShape()->setUserPointer((void*) ACTOR0);
	body[0].rigidBody = groundRigidBody;
	groundRigidBody->setRestitution(0.7);
	groundRigidBody->setFriction(0.1f);
	groundRigidBody->setHitFraction(0.01f);
	groundRigidBody->setActivationState(DISABLE_DEACTIVATION);
    dynamicsWorld->addRigidBody(groundRigidBody);

	
	//TRIGGER: TriggerRigidBody
    triggerMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(-8,-4,-10)));
    btRigidBody::btRigidBodyConstructionInfo triggerRigidBodyCI2(0,triggerMotionState,triggerShape,btVector3(0,0,0));
    triggerRigidBody = new btRigidBody(triggerRigidBodyCI2);
	triggerRigidBody->getCollisionShape()->setUserPointer((void*) ACTOR3);
	body[3].rigidBody = triggerRigidBody;
	triggerRigidBody->setCollisionFlags(triggerRigidBody->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
	//triggerRigidBody->setCollisionFlags(triggerRigidBody->getCollisionFlags() | btCollisionObject::CF_NO_CONTACT_RESPONSE);
	triggerRigidBody->setActivationState(DISABLE_DEACTIVATION);
	dynamicsWorld->addRigidBody(triggerRigidBody); 
 

	//FALL SPHERE: FallRigidBody
    fallMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(-12,5,-20)));
    btScalar mass = 1;
    btVector3 fallInertia(0,0,0);
    fallShape->calculateLocalInertia(mass,fallInertia);
    btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(mass,fallMotionState,fallShape,fallInertia);
    fallRigidBody = new btRigidBody(fallRigidBodyCI);
	fallRigidBody->getCollisionShape()->setUserPointer((void*) ACTOR1);
	body[1].rigidBody = fallRigidBody;
	fallRigidBody->setCollisionFlags(fallRigidBody->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
	fallRigidBody->setRestitution(0.5);
	fallRigidBody->setActivationState(DISABLE_DEACTIVATION);
	dynamicsWorld->addRigidBody(fallRigidBody);

	
	//CHARACTER: FallRigidBody2
    fallMotionState2 = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(-15,0,-30)));
	btScalar mass2 = 50;
    btVector3 fallInertia2(0,0,0);
	fallShape2->calculateLocalInertia(mass2,fallInertia2);
    btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI2(mass2,fallMotionState2,fallShape2,fallInertia2);
    fallRigidBody2 = new btRigidBody(fallRigidBodyCI2);
	fallRigidBody2->getCollisionShape()->setUserPointer((void*) ACTOR2);
	body[2].rigidBody = fallRigidBody2;
	fallRigidBody2->setCollisionFlags(fallRigidBody2->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
	fallRigidBody->setRestitution(0.5);
	//fallRigidBody2->setCollisionFlags(fallRigidBody2->getCollisionFlags() | btCollisionObject::CF_NO_CONTACT_RESPONSE);
	fallRigidBody2->setActivationState(DISABLE_DEACTIVATION);
	dynamicsWorld->addRigidBody(fallRigidBody2);
	
}	



int MyBullet::motionPhysics(void *data)
{
		btTransform trans;
		btTransform trans2;
		btTransform trans3;

		fallRigidBody->getMotionState()->getWorldTransform(trans);
		fallRigidBody2->getMotionState()->getWorldTransform(trans2);
		triggerRigidBody->getMotionState()->getWorldTransform(trans3);

		//fallRigidBody2->translate(btVector3(s_eye[0],0,s_eye[2]));
		fallRigidBody2->translate(btVector3(x_force,y_force,z_force));
		//fallRigidBody2->applyCentralForce(btVector3(x_force,y_force,z_force));

		x_force = 0;
		y_force = 0;
		z_force = 0;
		fallRigidBody->getMotionState()->setWorldTransform(fallRigidBody->getCenterOfMassTransform());
		fallRigidBody2->getMotionState()->setWorldTransform(fallRigidBody2->getCenterOfMassTransform());

		//dynamicsWorld->stepSimulation(1/60.f,10);
		dynamicsWorld->stepSimulation(1.0/60.0,10);
		dynamicsWorld->debugDrawWorld();

		//Solid Sphere
		glPushMatrix();
		glTranslatef(trans.getOrigin().getX(), trans.getOrigin().getY(), trans.getOrigin().getZ());
		glutSolidSphere(0.5, 32, 32);
		glPopMatrix();


		//Controlller: MD2 model 
		glEnable(GL_TEXTURE_2D);
		float percent = 0.03f;
		glPushMatrix();
		glTranslatef(trans2.getOrigin().getX(), trans2.getOrigin().getY()+1.5, trans2.getOrigin().getZ());
		glPushMatrix();
		glScalef(0.08,0.1,0.08);
		glRotatef(90.0f, -1.0f, 0.0f, 0.0f);
		glRotatef(90.0f, 0.0f, 0.0f, -1.0f);
		//myModel->SetState(MODEL_IDLE);
		switch (myModel->GetState())
		{
		case MODEL_IDLE:
			myModel->Animate(0, 39, percent);
			gunModel->Animate(0, 39, percent);
			break;
		case MODEL_RUN:
			myModel->Animate(40, 46, percent);
			gunModel->Animate(40, 46, percent);
			break;
		case MODEL_CROUCH:
			myModel->Animate(136, 154, percent);
			gunModel->Animate(136, 154, percent);
			break;
		case MODEL_JUMP:
			myModel->Animate(67, 73, percent);
			gunModel->Animate(67, 73, percent);
			break;
		default:
			break;
		}
		glPopMatrix();
		glPopMatrix();
		glDisable(GL_TEXTURE_2D);
		/*
		glPushMatrix();
		glTranslatef(trans2.getOrigin().getX(), trans2.getOrigin().getY()+1, trans2.getOrigin().getZ());
		glPushMatrix();
		static int n = 0;
		static float interp = 0.0;
		static double current_time = 0;
		static double last_time = 0;
		last_time = current_time;
		current_time = (double)glutGet (GLUT_ELAPSED_TIME) / 1000.0;
		interp += 10 * (current_time - last_time);
		//MD2Models::MyMD2Models::Animate (0, md2file.header.num_frames - 1, &n, &interp);
		if(modelState)
			MD2Models::MyMD2Models::Animate (40, 46, &n, &interp); //stop
		else
			MD2Models::MyMD2Models::Animate (1, 39, &n, &interp); //run
		glScalef(0.05,0.05,0.05);
		glRotatef(-90,0,1,0);
		glRotatef(-90,1,0,0);
		MD2Models::MyMD2Models::RenderFrameItp(n,interp,&md2file);
		modelState = false;
		glPopMatrix();
		glPopMatrix();
		*/


		//Collada model
		glEnable(GL_TEXTURE_2D);
		glBindTexture(GL_TEXTURE_2D, texture4);
		glEnable(GL_TEXTURE_GEN_S);
		glEnable(GL_TEXTURE_GEN_T);
		glTexGeni(GL_S, GL_TEXTURE_GEN_MODE, GL_OBJECT_LINEAR);
		glTexGeni(GL_T, GL_TEXTURE_GEN_MODE, GL_OBJECT_LINEAR);
		glPushMatrix();
		glTranslatef(trans3.getOrigin().getX(), trans3.getOrigin().getY()-1, trans3.getOrigin().getZ());
		glPushMatrix();
		glRotatef(-180,0,1,0);
		glRotatef(-90,1,0,0);
		Collada::MyCollada::RenderCollada(&colladafile);
		glPopMatrix();
		glPopMatrix();
		glDisable(GL_TEXTURE_GEN_S);
		glDisable(GL_TEXTURE_GEN_T);
		glDisable(GL_TEXTURE_2D);


		//platform
		glEnable(GL_TEXTURE_2D);
		glBindTexture(GL_TEXTURE_2D, texture3);
		glPushMatrix();
		glRotatef(30,0,1,0);
		glBegin(GL_QUADS);
			glTexCoord2f(0.0f, 0.0f);
			glVertex3f(-50,-5,-50);
			glTexCoord2f(6.0f, 0.0f);
			glVertex3f(-50,-5,50);
			glTexCoord2f(6.0f, 6.0f);
			glVertex3f(50,-5,50);
			glTexCoord2f(0.0f, 6.0f);
			glVertex3f(50,-5,-50);
		glEnd();
		glPopMatrix();
		glDisable(GL_TEXTURE_2D);

		return 0;
}



void MyBullet::cleanPhysics()
{	
		dynamicsWorld->removeRigidBody(fallRigidBody);
        delete fallRigidBody->getMotionState();
        delete fallRigidBody;
 
        dynamicsWorld->removeRigidBody(groundRigidBody);
        delete groundRigidBody->getMotionState();
        delete groundRigidBody;

        dynamicsWorld->removeRigidBody(triggerRigidBody);
        delete triggerRigidBody->getMotionState();
        delete triggerRigidBody;

        delete fallShape;
        delete triggerShape; 
        delete groundShape;
 
        delete dynamicsWorld;
        delete solver;
        delete collisionConfiguration;
        delete dispatcher;
        delete broadphase;
}



//Contact callback
bool MyBullet::CustomMaterialCombinerCallback(btManifoldPoint& cp,const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1)
{
	int obj0 = (int) (colObj0->getCollisionShape()->getUserPointer());
	int obj1 = (int) (colObj1->getCollisionShape()->getUserPointer());
	
	//no callback for ground collision
	if(obj1 == 0 || obj0 == 0){ }
	// use user pointer to recognise objects
	else if(!collisionFlag)
	{
		/*
		switch (obj0) 
		{
			case ACTOR1:
			{ 
				collisionFlag = true;
				cout<<"Collision occurred between "<<obj0<<" and "<<obj1<<endl;
				break;
			}
			case ACTOR2:
			{
				collisionFlag = true;
				cout<<"Collision occurred between "<<obj0<<" and "<<obj1<<endl;
				break;
			}
			case ACTOR3:
			{
				collisionFlag = true;
				cout<<"Collision occurred between "<<obj0<<" and "<<obj1<<endl;
				break;
			}			
			default:
				break;
		}
		*/
		//if touched the trigger
		if(obj0 == ACTOR3)
		{
			body[obj1].rigidBody->translate(btVector3(-10,10,-10)); 
			Mix_PlayChannel( -1, wave, 0 );
		}
		else if(obj1 == ACTOR3)
		{
			body[obj0].rigidBody->translate(btVector3(-10,10,-10)); 
			Mix_PlayChannel( -1, wave, 0 );
		}
		else { 	}
	}

	return true;
}



}